#include "GantryAgent.h"
#include "ScopedLock.h"

	//constructor

	/**
	 * @fn	GantryAgent::GantryAgent(std::string myname,FeederAgent* feed1,FeederAgent* feed2,
	 * 		FeederAgent* feed3,FeederAgent* feed4, GUIGantry* gui, int numPartsBin )
	 *
	 * @brief	Constructor.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param	myname		 	The myname.
	 * @param [in,out]	feed1	If non-null, the first feed.
	 * @param [in,out]	feed2	If non-null, the second feed.
	 * @param [in,out]	feed3	If non-null, the third feed.
	 * @param [in,out]	feed4	If non-null, the fourth feed.
	 * @param [in,out]	gui  	If non-null, the graphical user interface.
	 * @param	numPartsBin  	Number of parts bins.
	 */

	GantryAgent::GantryAgent(std::string myname):Agent(myname){
		for(int i=0;i<8;i++){
			enum PartType m;
			m=EMPTY;
			binLocations.push_back(m);
		}
		gantryOrders = new std::queue<GantryUnit>();
		isWaiting=false;
		this->timer = AwakeThread(this);
		purgeFeeder=false;
		explode=false;
		//waitingStatus= Sema(0);
	}

	//mesages

	/**
	 * @fn	void GantryAgent::msgPleaseGetParts(PartRecipe newPart, FeederAgent* newFeeder)
	 *
	 * @brief	Message please get parts.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param	newPart			 	The new part.
	 * @param [in,out]	newFeeder	If non-null, the new feeder.
	 */
	void GantryAgent::msgExplode(){
		explode =true;
	}
	void GantryAgent::msgPleaseGetParts(PartRecipe newPart, Feeder* newFeeder)
	 { 	//called by Feeder

		

		int feederNumb=0;
		//temp.partRecipe= newPart;
		if(newFeeder==feeder1){
			feederNumb=0;
		} else if(newFeeder==feeder2){
			feederNumb=1;
		}else if(newFeeder==feeder3){
			feederNumb=2;
		}else if(newFeeder==feeder4){
			feederNumb=3;
		}	
			FeederOrder temp(newPart,feederNumb);
			GantryUnit unit(temp,NEW);
		{
			ScopedLock scopedGantryOrdersListLock (&gantryOrdersLock);
			gantryOrders->push(unit);
		
		}

		print(this->toString() + " has recieved message msgPleaseGetParts");
		stateChanged();
	}

	/**
	 * @fn	void GantryAgent::msgGUIHalfDoneMoving(FeederOrder finishedOrder)
	 *
	 * @brief	Message graphical user interface half done moving.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param	finishedOrder	The finished order.
	 */
	
	void GantryAgent::msgGUIHalfDoneMoving(int num)
	{
		
		{
			ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
			gantryOrders->front().state=TELLFEEDER;
			setBin(NONE,num);
		}
		print(this->toString() + " has recieved message msgGUIHalfDoneMoving");
		stateChanged();
	}
	void GantryAgent::msgPurgeFeeder(){
		purgeFeeder=true;
	}

	/**
	 * @fn	void GantryAgent::msgGUIDoneMoving()
	 *
	 * @brief	Message graphical user interface done moving.
	 *
	 * @author	Graham Matuszewski
	 */

	void GantryAgent::msgGUIDoneMoving()
	{		//called by GUIGantry
		//completedRecipes.push(finishedOrder);
		{
			ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
			if(gantryOrders->size()>0)	
				gantryOrders->front().state=MOVE;
				//gantryOrders->front().state=NEW;
		}

		//either go to idle or keep moving
		print(this->toString() + " has recieved message msgGUIDoneMoving");
		stateChanged();
	}

	/**
	 * @fn	void GantryAgent::msgFeederStatus(FeederAgent* newFeeder,bool busy)
	 *
	 * @brief	Message feeder status.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param [in,out]	newFeeder	If non-null, the new feeder.
	 * @param	busy			 	true to busy.
	 */


	void GantryAgent::msgFeederStatus(Feeder* newFeeder,bool busy){
		isWaiting=false;
		{
			ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
			if(gantryOrders->size()>0)
				if(gantryOrders->front().state==CHECKSTATUS){
				gantryOrders->front().state=BIN;
			}

		}
		print(this->toString() + " has recieved message msgFeederStatus");
		stateChanged();
		//stateChanged();
	}

	/**
	 * @fn	bool GantryAgent::PickAndExecuteAnAction()
	 *
	 * @brief	Gets the pick and execute an action.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @return	true if it succeeds, false if it fails.
	 */

	bool GantryAgent::pickAndExecuteAnAction()
	{
			if(gantryOrders->size()>0){
				GantryUnit current= gantryOrders->front();
				switch(current.state){
					case TELLFEEDER:
						gantryOrders->front().state=IDLE;
						gantryDoneMovingSendParts(current.order);
						
						return true;
					case BIN: 
						
						moveToBin(current.order);
						return true;
					case MOVE:
						
						moveGantryToFeeder(current.order);
						return true;
					case NEW:
						
						checkStatusOfFeeder(current.order);
						return true;
					}
				if(purgeFeeder){
					feederJammed();
				}
			}
			else{
				goIdle();
			}



		return false;
	}

	/**
	 * @fn	void GantryAgent::checkStatusOfFeeder(FeederOrder newOrder)
	 *
	 * @brief	Check status of feeder.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param	newOrder	The new order.
	 */

	void GantryAgent::checkStatusOfFeeder(FeederOrder newOrder){

		gantryOrders->front().state=CHECKSTATUS;
		if(newOrder.feederNumber==0){
			feeder1->msgCurrentStatus();
		}else if(newOrder.feederNumber==1){
			feeder2->msgCurrentStatus();
		}else if(newOrder.feederNumber==2){
			feeder3->msgCurrentStatus();
		}else if(newOrder.feederNumber==3){
			feeder4->msgCurrentStatus();
		}
		isWaiting=true;
		waitingFeeder=newOrder.feederNumber;
		timer.start();
		//{
			//ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
		//}
		char buffer[2];
		Do(this->toString() + " Feeder Number" + itoa(newOrder.feederNumber,buffer,2) + "is being asked to give a status update");
	}

	/**
	 * @fn	void GantryAgent::moveGantryToFeeder(FeederOrder* newOrder)
	 *
	 * @brief	Move gantry to feeder.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param [in,out]	newOrder	If non-null, the new order.
	 */
	void GantryAgent::moveToBin(FeederOrder newOrder){
		bool worked=false;
		for(int i=0;i<binLocations.size();i++){
				if(binLocations[i]==EMPTY){
					gantryOrders->front().state=GUIBIN;
					guiGantry->DoGUIReplaceBin(newOrder.partRecipe.partType,i);
					//{
						//ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
						
					//}
					binLocations[i]=newOrder.partRecipe.partType;
					//FeederOrder f(newOrder.partRecipe,newOrder.feederNumber);
					//f.feederNumber=newOrder->feederNumber;
					//f.partRecipe= newOrder->partRecipe;
					//GUIworking=true;
					Do(this->toString() + " Has told the Gui to replace an empty bin " );
					worked=true;
					break;
				}
		}
		if(!worked){
			for(int i=0;i<binLocations.size();i++){
				if(binLocations[i]==NONE){
					gantryOrders->front().state=GUIBIN;
					guiGantry->DoGUIReplaceBin(newOrder.partRecipe.partType,i);
					//{
						//ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
						
					//}
					binLocations[i]=newOrder.partRecipe.partType;
					//FeederOrder f(newOrder.partRecipe,newOrder.feederNumber);
					//f.feederNumber=newOrder->feederNumber;
					//f.partRecipe= newOrder->partRecipe;
					//GUIworking=true;
					Do(this->toString() + " Has told the Gui to replace an empty bin " );
					worked=true;
					break;
				}
		}


		}


	}
	void GantryAgent::feederJammed(){
		purgeFeeder=false;
		isWaiting=false;
		if(waitingFeeder==0){
			feeder1->msgPurgeParts();
		}else if(waitingFeeder==1){
			feeder2->msgPurgeParts();
		}else if(waitingFeeder==2){
			feeder3->msgPurgeParts();
		}else if(waitingFeeder==3){
			feeder4->msgPurgeParts();
		}
		gantryOrders->front().state=CHECKSTATUS;
		


	}

	void GantryAgent::moveGantryToFeeder(FeederOrder newOrder)
	{
		enum PartType m = EMPTY;
		for(int i=0;i<binLocations.size();i++){
			if(binLocations[i]==newOrder.partRecipe.partType){
				guiGantry->DoGUIMoveToFeeder(i,newOrder.feederNumber,newOrder.partRecipe.topLane);
				binLocations[i]=newOrder.partRecipe.partType;
				//{
					//ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
					gantryOrders->front().state=ONWAYFEEDER;
				//}
				char buffer[2];
				Do(this->toString() + " Has told the Gui to begin moving towards feeder number " + itoa(newOrder.feederNumber,buffer,2) );
				break;
			}
		}

	}

	/**
	 * @fn	void GantryAgent::gantryDoneMovingSendParts(FeederOrder* newOrder)
	 *
	 * @brief	Gantry done moving send parts.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @param [in,out]	newOrder	If non-null, the new order.
	 */

	void GantryAgent::goIdle(){
		guiGantry->DoGoIdle();
	}


	void GantryAgent::gantryDoneMovingSendParts(FeederOrder newOrder)
	{
		int feederNumber = newOrder.feederNumber;
		if(feederNumber==0){
			feeder1->msgHereAreParts(newOrder.partRecipe);
		}else if(feederNumber==1){
			feeder2->msgHereAreParts(newOrder.partRecipe);
		}else if(feederNumber==2){
			feeder3->msgHereAreParts(newOrder.partRecipe);
		}else if(feederNumber==3){
			feeder4->msgHereAreParts(newOrder.partRecipe);
		}
		{
			ScopedLock scopedGantryOrdersListLock(&gantryOrdersLock);
			gantryOrders->pop();
		}
		char buffer[2];
		stateChanged();
		//Do(this->toString() + " Telling feeder numer " + itoa(newOrder->feederNumber,buffer,2)+ " that they have recieved parts" );
				
	}

	/**
	 * @fn	std::string FeederAgent::toString() const
	 *
	 * @brief	Convert this object into a string representation.
	 *
	 * @author	Graham Matuszewski
	 *
	 * @return	A string representation of this object.
	 */

	std::string GantryAgent::toString() const
	{
		return "GantryAgent " + getName();
	}

	void GantryAgent::setFeeder1(Feeder* feed1){
		feeder1=feed1;
	}
	void GantryAgent::setFeeder2(Feeder* feed2){
		feeder2=feed2;
	}
	void GantryAgent::setFeeder3(Feeder* feed3){
		feeder3=feed3;
	}
	void GantryAgent::setFeeder4(Feeder* feed4){
		feeder4=feed4;
	}
	void GantryAgent::setGuiGantry(GUIGantry* hey){
		guiGantry=hey;
	}

	GUIGantry* GantryAgent::getGUI(){
		return guiGantry;
	}


	void GantryAgent::setStartBins(std::vector<PartType> parts){
		for(int i=0;i<8;i++){
			binLocations[i]=parts[i];
		}
	}

	void GantryAgent::setBin(PartType part,int bin){
		binLocations[bin]=part;
	}

	void GantryAgent::handleBreak(){
		guiGantry->DoBreak();
		if(explode){
			guiGantry->DoExplode();
			explode = false;
		}
	}
	void GantryAgent::handleFix(){
		guiGantry->DoFix();
	}


	Feeder* GantryAgent::getFeeder1(){ return feeder1;}
	Feeder* GantryAgent::getFeeder2(){ return feeder2;}
	Feeder* GantryAgent::getFeeder3(){ return feeder3;}
	Feeder* GantryAgent::getFeeder4(){return feeder4;}